#!/usr/bin/env python

import rospy
import math

import actionlib
from move_base_msgs.msg import MoveBaseAction, MoveBaseGoal
from actionlib_msgs.msg import GoalStatus
from geometry_msgs.msg import Pose, Point, Quaternion
from tf.transformations import quaternion_from_euler
import random


class MoveBaseSeq():

    def __init__(self):

        rospy.init_node('move_base_sequence')
        self.goal_cnt = 0
        #Create action client
        self.client = actionlib.SimpleActionClient('move_base',MoveBaseAction)
        rospy.loginfo("Waiting for move_base action server...")
        #wait = self.client.wait_for_server(rospy.Duration(5.0))
        wait = self.client.wait_for_server()
        if not wait:
            rospy.logerr("Action server not available!")
            rospy.signal_shutdown("Action server not available!")
            return
        rospy.loginfo("Connected to move base server")
        rospy.loginfo("Starting goals achievements ...")
        # self.movebase_client()
        self.movebase_client_forever()

    def active_cb(self):
        rospy.loginfo("Goal pose "+str(self.goal_cnt+1)+" is now being processed by the Action Server...")

    def feedback_cb(self, feedback):
        #rospy.loginfo("Feedback for goal "+str(self.goal_cnt)+": "+str(feedback))
        rospy.loginfo("Feedback for goal pose "+str(self.goal_cnt+1)+" received")

    def done_cb(self, status, result):
        self.goal_cnt += 1
    # Reference for terminal status values: http://docs.ros.org/diamondback/api/actionlib_msgs/html/msg/GoalStatus.html
        if status == 2:
            rospy.loginfo("Goal pose "+str(self.goal_cnt)+" received a cancel request after it started executing, completed execution!")

        if status == 3:
            rospy.loginfo("Goal pose "+str(self.goal_cnt)+" reached") 
            next_goal = self.generate_random_goal()
            rospy.loginfo("Sending goal pose "+str(self.goal_cnt+1)+" to Action Server")
            self.client.send_goal(next_goal, self.done_cb, self.active_cb, self.feedback_cb) 

        if status == 4:
            rospy.loginfo("Goal pose "+str(self.goal_cnt)+" was aborted by the Action Server")
            rospy.signal_shutdown("Goal pose "+str(self.goal_cnt)+" aborted, shutting down!")
            return

        if status == 5:
            rospy.loginfo("Goal pose "+str(self.goal_cnt)+" has been rejected by the Action Server")
            rospy.signal_shutdown("Goal pose "+str(self.goal_cnt)+" rejected, shutting down!")
            return

        if status == 8:
            rospy.loginfo("Goal pose "+str(self.goal_cnt)+" received a cancel request before it started executing, successfully cancelled!")

    # Custom made by Hansen
    def generate_random_goal(self):
        generated_goal = MoveBaseGoal()
        
        random_side = random.randint(1, 4)
        if random_side==1:
            x = random.randint(45, 60)/10
            y = random.randint(-30, 30)/10
        elif random_side==2:
            x = random.randint(-10, 10)/10
            y = random.randint(-20, -15)/10
        elif random_side==3:
            x = random.randint(-20, -15)/10
            y = random.randint(-10, 10)/10
        elif random_side==4:
            x = random.randint(-10, 10)/10
            y = random.randint(15, 20)/10

        generated_goal.target_pose.header.frame_id = 'map'
        generated_goal.target_pose.pose.position.x = x
        generated_goal.target_pose.pose.position.y = y
        generated_goal.target_pose.pose.position.z = 0.0
        generated_goal.target_pose.pose.orientation.x = 0
        generated_goal.target_pose.pose.orientation.y = 0
        generated_goal.target_pose.pose.orientation.z = 0
        generated_goal.target_pose.pose.orientation.w = 1
        return generated_goal
        
    # Custom made by Hansen
    def movebase_client_forever(self):
        while True:
            goal = self.generate_random_goal()
            print(goal.target_pose.pose)
            rospy.loginfo("Sending goal pose "+str(self.goal_cnt+1)+" to Action Server")
            self.client.send_goal(goal, self.done_cb, self.active_cb, self.feedback_cb)
            rospy.spin()

if __name__ == '__main__':
    try:
        MoveBaseSeq()
    except rospy.ROSInterruptException:
        rospy.loginfo("Navigation finished.")
